#!/bin/bash

# 颜色校正系统测试脚本

echo "=== 颜色校正系统集成测试 ==="

# 检查参数
if [ $# -lt 1 ]; then
    echo "用法: $0 <测试模式> [参数...]"
    echo ""
    echo "测试模式:"
    echo "  setup         - 编译和设置环境"
    echo "  calibrate     - 校准测试 <参考图像> <输入图像> [配置文件]"
    echo "  apply         - 应用校正测试 <输入图像> [配置文件] [输出图像]"
    echo "  mcu_trigger   - 模拟MCU触发颜色校正"
    echo "  mcu_monitor   - 监控MCU通信"
    echo "  mcu_reset     - 重置MCU信号"
    echo "  system_test   - 完整系统测试"
    echo "  topics        - 查看相关话题"
    echo "  services      - 查看相关服务"
    exit 1
fi

# 设置环境
cd /home/firefly/ws_vision
source devel/setup.bash

MODE=$1

case $MODE in
    "setup")
        echo "1. 编译颜色校正包..."
        catkin_make --pkg color_correction
        if [ $? -eq 0 ]; then
            echo "✓ 编译成功"
            source devel/setup.bash
            echo "✓ 环境设置完成"
        else
            echo "✗ 编译失败"
            exit 1
        fi
        ;;
        
    "calibrate")
        if [ $# -lt 3 ]; then
            echo "校准测试需要参数: calibrate <参考图像> <输入图像> [配置文件]"
            exit 1
        fi
        
        REFERENCE=$2
        INPUT=$3
        CONFIG=${4:-"$(rospack find color_correction)/config/color_correction_params.json"}
        
        echo "开始校准测试..."
        echo "参考图像: $REFERENCE"
        echo "输入图像: $INPUT"
        echo "配置文件: $CONFIG"
        
        if [ ! -f "$REFERENCE" ]; then
            echo "✗ 参考图像不存在: $REFERENCE"
            exit 1
        fi
        
        if [ ! -f "$INPUT" ]; then
            echo "✗ 输入图像不存在: $INPUT"
            exit 1
        fi
        
        # 启动节点
        echo "启动颜色校正节点..."
        roslaunch color_correction color_correction.launch &
        LAUNCH_PID=$!
        sleep 3
        
        # 执行校准
        echo "执行校准..."
        rosrun color_correction color_correction_client calibrate "$REFERENCE" "$INPUT" "$CONFIG"
        
        # 清理
        kill $LAUNCH_PID 2>/dev/null
        echo "校准测试完成"
        ;;
        
    "apply")
        if [ $# -lt 2 ]; then
            echo "应用测试需要参数: apply <输入图像> [配置文件] [输出图像]"
            exit 1
        fi
        
        INPUT=$2
        CONFIG=${3:-"$(rospack find color_correction)/config/color_correction_params.json"}
        OUTPUT=${4:-"/tmp/corrected_output.jpg"}
        
        echo "开始应用校正测试..."
        echo "输入图像: $INPUT"
        echo "配置文件: $CONFIG"
        echo "输出图像: $OUTPUT"
        
        if [ ! -f "$INPUT" ]; then
            echo "✗ 输入图像不存在: $INPUT"
            exit 1
        fi
        
        if [ ! -f "$CONFIG" ]; then
            echo "✗ 配置文件不存在: $CONFIG"
            exit 1
        fi
        
        # 启动节点
        echo "启动颜色校正节点..."
        roslaunch color_correction color_correction.launch &
        LAUNCH_PID=$!
        sleep 3
        
        # 执行应用
        echo "执行颜色校正..."
        rosrun color_correction color_correction_client apply "$INPUT" "$CONFIG" "$OUTPUT"
        
        if [ -f "$OUTPUT" ]; then
            echo "✓ 校正图像已保存到: $OUTPUT"
        else
            echo "✗ 校正图像生成失败"
        fi
        
        # 清理
        kill $LAUNCH_PID 2>/dev/null
        echo "应用测试完成"
        ;;
        
    "mcu_trigger")
        echo "模拟MCU触发颜色校正..."
        
        # 检查是否有节点运行
        if ! pgrep -f "color_correction_node" > /dev/null; then
            echo "启动颜色校正节点..."
            roslaunch color_correction color_correction.launch &
            LAUNCH_PID=$!
            sleep 3
        fi
        
        echo "发送颜色校正触发信号..."
        rosrun color_correction mcu_communication_tester --trigger
        
        echo "等待处理完成..."
        sleep 2
        
        echo "检查结果..."
        rosrun color_correction mcu_communication_tester --status
        
        # 如果启动了节点，询问是否关闭
        if [ ! -z "$LAUNCH_PID" ]; then
            echo ""
            read -p "是否关闭测试节点? (y/n): " -n 1 -r
            echo
            if [[ $REPLY =~ ^[Yy]$ ]]; then
                kill $LAUNCH_PID
                echo "节点已关闭"
            fi
        fi
        ;;
        
    "mcu_monitor")
        echo "启动MCU通信监控..."
        
        # 检查是否有节点运行
        if ! pgrep -f "color_correction_node" > /dev/null; then
            echo "启动颜色校正节点..."
            roslaunch color_correction color_correction.launch &
            LAUNCH_PID=$!
            sleep 3
        fi
        
        echo "开始监控MCU信号... (按Ctrl+C停止)"
        rosrun color_correction mcu_communication_tester --monitor
        
        # 如果启动了节点，关闭它
        if [ ! -z "$LAUNCH_PID" ]; then
            kill $LAUNCH_PID 2>/dev/null
            echo "节点已关闭"
        fi
        ;;
        
    "mcu_reset")
        echo "重置MCU信号..."
        rosrun color_correction mcu_communication_tester --reset
        echo "MCU信号已重置"
        ;;
        
    "system_test")
        echo "开始完整系统测试..."
        
        # 启动完整系统
        echo "1. 启动颜色校正系统..."
        roslaunch color_correction color_correction_auto.launch &
        LAUNCH_PID=$!
        sleep 5
        
        # 检查节点状态
        echo "2. 检查节点状态..."
        rosnode list | grep color_correction
        
        # 检查话题
        echo "3. 检查话题..."
        rostopic list | grep -E "(corrected|color_correction)"
        
        # 检查服务
        echo "4. 检查服务..."
        rosservice list | grep color_correction
        
        # 模拟MCU触发
        echo "5. 模拟MCU触发..."
        sleep 2
        rosrun color_correction mcu_communication_tester --trigger
        
        echo "6. 系统测试完成"
        
        # 询问是否关闭
        echo ""
        read -p "是否关闭系统? (y/n): " -n 1 -r
        echo
        if [[ $REPLY =~ ^[Yy]$ ]]; then
            kill $LAUNCH_PID
            echo "系统已关闭"
        fi
        ;;
        
    "topics")
        echo "相关ROS话题:"
        rostopic list | grep -E "(image|color_correction|serial)" | sort
        echo ""
        echo "图像话题信息:"
        for topic in $(rostopic list | grep -E "image.*corrected"); do
            echo "  $topic:"
            rostopic info "$topic" 2>/dev/null | grep -E "(Type|Publishers|Subscribers)" | sed 's/^/    /'
        done
        ;;
        
    "services")
        echo "相关ROS服务:"
        rosservice list | grep color_correction
        echo ""
        echo "服务接口:"
        for service in $(rosservice list | grep color_correction); do
            echo "  $service:"
            rossrv show "$(rosservice type $service)" | sed 's/^/    /'
            echo ""
        done
        ;;
        
    *)
        echo "未知测试模式: $MODE"
        echo "使用 '$0' 查看可用模式"
        exit 1
        ;;
esac

echo "测试完成"
